
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_airspeed.c
  * @author     baiyang
  * @date       2023-8-1
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_airspeed.h"
#include "sensor_airspeed_ms4525.h"
#include "sensor_airspeed_ms5525.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <logger/ap_log.h>
#include <gcs_mavlink/gcs.h>
#include <parameter/param.h>
#include <common/time/gp_time.h>
#include <vehicle/vehicle_type.h>
#include <srv_channel/srv_channel.h>
/*-----------------------------------macro------------------------------------*/
#define ENABLE_PARAMETER !(MB_BUILD_TYPE(MICROBEE_BUILD_Plane))
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static struct sensor_airspeed sensor_as;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
 * Get the sensor_baro singleton
 */
sensor_airspeed_t sensor_as_get_singleton()
{
    return &sensor_as;
}

void sensor_as_ctor()
{
    sensor_as_assign_param();

    for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
        airspeed_calibration_ctor(&sensor_as.state[i].calibration);
    }

    sensor_as._log_bit = -1;     // stores which bit in LOG_BITMASK is used to indicate we should log airspeed readings
}

void sensor_as_init()
{
#if ENABLE_PARAMETER
    // Check if enabled
    if (!sensor_as_lib_enabled()) {
        return;
    }
#endif

    if (sensor_as_enabled2(0)) {
        sensor_as_allocate();
    }
}

void sensor_as_allocate()
{
    if (sensor_as.sensor[0] != NULL) {
        // already initialised, periph may call allocate several times to allow CAN detection
        return;
    }

    // look for sensors based on type parameters
    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
#if MB_AIRSPEED_AUTOCAL_ENABLE
        airspeed_calibration_init(&sensor_as.state[i].calibration, sensor_as.param[i].ratio);
        sensor_as.state[i].last_saved_ratio = sensor_as.param[i].ratio;
#endif

        // Set the enable automatically to false and set the probability that the airspeed is healhy to start with
        sensor_as.state[i].failures.health_probability = 1.0f;

        switch ((enum airspeed_type)sensor_as.param[i].type) {
        case AS_TYPE_NONE:
            // nothing to do
            break;
        case AS_TYPE_I2C_MS4525:
#if MB_AIRSPEED_MS4525_ENABLED
            sensor_as.sensor[i] = sensor_as_ms4525_probe(i);
#endif
            break;
        case AS_TYPE_I2C_MS5525:
#if MB_AIRSPEED_MS5525_ENABLED
            sensor_as.sensor[i] = sensor_as_ms5525_probe(i, MS5525_ADDR_AUTO);
#endif
            break;
        case AS_TYPE_I2C_MS5525_ADDRESS_1:
#if MB_AIRSPEED_MS5525_ENABLED
            sensor_as.sensor[i] = sensor_as_ms5525_probe(i, MS5525_ADDR_1);
#endif
            break;
        case AS_TYPE_I2C_MS5525_ADDRESS_2:
#if MB_AIRSPEED_MS5525_ENABLED
            sensor_as.sensor[i] = sensor_as_ms5525_probe(i, MS5525_ADDR_2);
#endif
            break;
        default:
            break;
        }
        if (sensor_as.sensor[i] && !sensor_as_backend_init(sensor_as.sensor[i])) {
            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Airspeed %u init failed", i + 1);
            sensor_as_backend_destructor(sensor_as.sensor[i]);
            rt_free(sensor_as.sensor[i]);
            sensor_as.sensor[i] = NULL;
        }
        if (sensor_as.sensor[i] != NULL) {
            sensor_as.num_sensors = i+1;
        }
    }

    // set DEVID to zero for any sensors not found. This allows backends to order
    // based on previous value of DEVID. This allows for swapping out sensors
    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
        if (sensor_as.sensor[i] == NULL) {
            // note we use set() not set_and_save() to allow a sensor to be temporarily
            // removed for one boot without losing its slot
            sensor_as.param[i].bus_id = 0;
        }
    }
}

// read one airspeed sensor
void sensor_as_read(uint8_t i)
{
    if (!sensor_as_enabled2(i) || !sensor_as.sensor[i]) {
        return;
    }
    sensor_as.state[i].last_update_us = time_micros64();

    // try and get a direct reading of airspeed
    if (sensor_as_backend_has_airspeed(sensor_as.sensor[i])) {
        sensor_as.state[i].healthy = sensor_as_backend_get_airspeed(sensor_as.sensor[i], &sensor_as.state[i].airspeed);
        sensor_as.state[i].raw_airspeed = sensor_as.state[i].airspeed;  // for logging
        return;
    }

    float raw_pressure = sensor_as_get_pressure(i);
    float airspeed_pressure = raw_pressure - sensor_as.param[i].offset;

    // remember raw pressure for logging
    sensor_as.state[i].corrected_pressure = airspeed_pressure;

    bool prev_healthy = sensor_as.state[i].healthy;
    if (sensor_as.state[i].cal.start_ms != 0) {
        sensor_as_update_calibration3(i, raw_pressure);
    }

    // filter before clamping positive
    if (!prev_healthy) {
        // if the previous state was not healthy then we should not
        // use an IIR filter, otherwise a bad reading will last for
        // some time after the sensor becomees healthy again
        sensor_as.state[i].filtered_pressure = airspeed_pressure;
    } else {
        sensor_as.state[i].filtered_pressure = 0.7f * sensor_as.state[i].filtered_pressure + 0.3f * airspeed_pressure;
    }

    /*
      we support different pitot tube setups so user can choose if
      they want to be able to detect pressure on the static port
     */
    switch ((enum airspeed_pitot_tube_order)sensor_as.param[i].tube_order) {
    case AS_PITOT_TUBE_ORDER_NEGATIVE:
        sensor_as.state[i].last_pressure  = -airspeed_pressure;
        sensor_as.state[i].raw_airspeed   = sqrtf(MAX(-airspeed_pressure, 0) * sensor_as.param[i].ratio);
        sensor_as.state[i].airspeed       = sqrtf(MAX(-sensor_as.state[i].filtered_pressure, 0) * sensor_as.param[i].ratio);
        break;
    case AS_PITOT_TUBE_ORDER_POSITIVE:
        sensor_as.state[i].last_pressure  = airspeed_pressure;
        sensor_as.state[i].raw_airspeed   = sqrtf(MAX(airspeed_pressure, 0) * sensor_as.param[i].ratio);
        sensor_as.state[i].airspeed       = sqrtf(MAX(sensor_as.state[i].filtered_pressure, 0) * sensor_as.param[i].ratio);
        break;
    case AS_PITOT_TUBE_ORDER_AUTO:
    default:
        sensor_as.state[i].last_pressure  = fabsf(airspeed_pressure);
        sensor_as.state[i].raw_airspeed   = sqrtf(fabsf(airspeed_pressure) * sensor_as.param[i].ratio);
        sensor_as.state[i].airspeed       = sqrtf(fabsf(sensor_as.state[i].filtered_pressure) * sensor_as.param[i].ratio);
        break;
    }
}

// read all airspeed sensors
void sensor_as_update()
{
    if (!sensor_as_lib_enabled()) {
        return;
    }

    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
        sensor_as_read(i);
    }

    // debugging until we get MAVLink support for 2nd airspeed sensor
    if (sensor_as_enabled2(1)) {
        gcs_send_named_float("AS2", sensor_as_get_airspeed2(1));
    }

    const uint8_t old_primary = sensor_as.primary;

    // setup primary
    if (sensor_as_healthy2(sensor_as.primary_sensor)) {
        sensor_as.primary = sensor_as.primary_sensor;
    } else {
        for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
            if (sensor_as_healthy2(i)) {
                sensor_as.primary = i;
                break;
            }
        }
    }

    sensor_as_check_sensor_failures();

    if (sensor_as.primary != old_primary) {
        aplog_write_event(LOG_EVENT_AIRSPEED_PRIMARY_CHANGED);
    }
#if 0
    if (sensor_as._log_bit != (uint32_t)-1 && AP::logger().should_log(_log_bit)) {
        Log_Airspeed();
    }
#endif

    uitc_sensor_airspeed_raw airspeed_raw;
    uitc_sensor_airspeed airspeed;

    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
        float temperature;
        if (!sensor_as_get_temperature2(i, &temperature)) {
            temperature = 0;
        }

        airspeed_raw.timestamp_us = sensor_as.state[i].last_update_us;
        airspeed_raw.raw_airspeed = sensor_as_get_raw_airspeed2(i);
        airspeed_raw.airspeed     = sensor_as_get_airspeed2(i);
        airspeed_raw.diffpressure = sensor_as.state[i].last_pressure;
        airspeed_raw.temperature = temperature;
        airspeed_raw.rawpressure = sensor_as.state[i].corrected_pressure;
        airspeed_raw.offset      = sensor_as.param[i].offset;
        airspeed_raw.use         = sensor_as_use2(i);
        airspeed_raw.healthy     = sensor_as_healthy2(i);
        airspeed_raw.health_prob = sensor_as.state[i].failures.health_probability;
        airspeed_raw.test_ratio  = sensor_as.state[i].failures.test_ratio;
        airspeed_raw.primary     = sensor_as.primary;

        if (i == 0) {
            itc_publish(ITC_ID(sensor_airspeed_raw1), &airspeed_raw);
        } else if (i == 1) {
            itc_publish(ITC_ID(sensor_airspeed_raw2), &airspeed_raw);
        }

        if (i == sensor_as.primary) {
            airspeed.timestamp_us = airspeed_raw.timestamp_us;
            airspeed.raw_airspeed = airspeed_raw.raw_airspeed;
            airspeed.airspeed     = airspeed_raw.airspeed;
            airspeed.temperature  = airspeed_raw.temperature;
            airspeed.health_prob  = airspeed_raw.health_prob;
            airspeed.test_ratio   = airspeed_raw.test_ratio;
            airspeed.primary      = airspeed_raw.primary;
            airspeed.healthy      = airspeed_raw.healthy;

            itc_publish(ITC_ID(sensor_airspeed), &airspeed_raw);
        }
    }
}

/*
  update async airspeed zero offset calibration
*/
void sensor_as_update_calibration3(uint8_t i, float raw_pressure)
{
    if (!sensor_as_enabled2(i) || sensor_as.state[i].cal.start_ms == 0) {
        return;
    }
    
    // consider calibration complete when we have at least 15 samples
    // over at least 1 second
    if (time_millis() - sensor_as.state[i].cal.start_ms >= 1000 &&
        sensor_as.state[i].cal.read_count > 15) {
        if (sensor_as.state[i].cal.count == 0) {
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Airspeed %u unhealthy", i + 1);
            sensor_as.calibration_state[i] = AS_CAL_STATE_FAILED;
        } else {
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u calibrated", i + 1);
            float calibrated_offset = sensor_as.state[i].cal.sum / sensor_as.state[i].cal.count;
            // check if new offset differs too greatly from last calibration, indicating pitot uncovered in wind
            if (sensor_as.fixed_wing_parameters != NULL) {
                float airspeed_min = sensor_as.fixed_wing_parameters->airspeed_min;
                // use percentage of ARSPD_FBW_MIN as criteria for max allowed change in offset
                float max_change = 0.5*(sq((1 + (sensor_as.max_speed_pcnt * 0.01))*airspeed_min) - sq(airspeed_min));
                if (sensor_as.max_speed_pcnt > 0 && (fabsf(calibrated_offset-sensor_as.param[i].offset) > max_change) && (fabsf(sensor_as.param[i].offset) > 0)) {
                    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Arspd %d offset change large;cover and recal", i +1);
                }
            }

            param_t *param = param_get_by_variable2("ARSPD", (void*)&sensor_as.param[i].offset);
            if (param != NULL) {
                param_set_and_save(param, calibrated_offset);
            }

            sensor_as.calibration_state[i] = AS_CAL_STATE_SUCCESS;
        }
        sensor_as.state[i].cal.start_ms = 0;
        return;
    }
    // we discard the first 5 samples
    if (sensor_as.state[i].healthy && sensor_as.state[i].cal.read_count > 5) {
        sensor_as.state[i].cal.sum += raw_pressure;
        sensor_as.state[i].cal.count++;
    }
    sensor_as.state[i].cal.read_count++;
}

bool sensor_as_lib_enabled()
{
#if ENABLE_PARAMETER
    return sensor_as._enable > 0;
#endif
    return true;
}

bool sensor_as_enabled(void)
{
    return sensor_as_enabled2(sensor_as.primary);
}

// return true if airspeed is enabled
bool sensor_as_enabled2(uint8_t i)
{
    if (!sensor_as_lib_enabled()) {
        return false;
    }
    if (i < AIRSPEED_MAX_SENSORS) {
        return sensor_as.param[i].type != AS_TYPE_NONE;
    }
    return false;
}

// return the differential pressure in Pascal for the last airspeed reading
float sensor_as_get_differential_pressure(uint8_t i)
{
    if (!sensor_as_enabled2(i)) {
        return 0.0f;
    }
    return sensor_as.state[i].last_pressure;
}

// read the airspeed sensor
float sensor_as_get_pressure(uint8_t i)
{
    if (!sensor_as_enabled2(i)) {
        return 0;
    }
    float pressure = 0;
    if (sensor_as.sensor[i]) {
        sensor_as.state[i].healthy = sensor_as_backend_get_differential_pressure(sensor_as.sensor[i], &pressure);
    }
    return pressure;
}

bool sensor_as_get_temperature(float *temperature)
{
    return sensor_as_get_temperature2(sensor_as.primary, temperature);
}

// get a temperature reading if possible
bool sensor_as_get_temperature2(uint8_t i, float *temperature)
{
    if (!sensor_as_enabled2(i)) {
        return false;
    }
    if (sensor_as.sensor[i]) {
        return sensor_as_backend_get_temperature(sensor_as.sensor[i], temperature);
    }
    return false;
}

float sensor_as_get_airspeed(void)
{
    return sensor_as_get_airspeed2(sensor_as.primary);
}

// return the current airspeed in m/s
float sensor_as_get_airspeed2(uint8_t i)
{
    if (!sensor_as_enabled2(i)) {
        // we can't have negative airspeed so sending an obviously invalid value
        return -1.0;
    }
    return sensor_as.state[i].airspeed;
}

float sensor_as_get_raw_airspeed(void)
{
    return sensor_as_get_raw_airspeed2(sensor_as.primary);
}

// return the unfiltered airspeed in m/s
float sensor_as_get_raw_airspeed2(uint8_t i)
{
    if (!sensor_as_enabled2(i)) {
        // we can't have negative airspeed so sending an obviously invalid value
        return -1.0;
    }
    return sensor_as.state[i].raw_airspeed;
}

bool sensor_as_healthy(void)
{
    return sensor_as_healthy2(sensor_as.primary);
}

// return health status of sensor
bool sensor_as_healthy2(uint8_t i)
{
    bool ok = sensor_as.state[i].healthy && sensor_as_enabled2(i) && sensor_as.sensor[i] != NULL;

    // sanity check the offset parameter.  Zero is permitted if we are skipping calibration.
    ok &= (fabsf(sensor_as.param[i].offset) > 0 || sensor_as.state[i].use_zero_offset || sensor_as.param[i].skip_cal);

    return ok;
}

// return time in ms of last update
uint64_t sensor_as_last_update_us(void)
{
    return sensor_as_last_update_us2(sensor_as.primary);
}

uint64_t sensor_as_last_update_us2(uint8_t i)
{
    return sensor_as.state[i].last_update_us;
}

bool sensor_as_use()
{
    return sensor_as_use2(sensor_as.primary);
}

bool sensor_as_use2(uint8_t i)
{
    if (!sensor_as_lib_enabled()) {
        return false;
    }
    if (sensor_as._force_disable_use) {
        return false;
    }
    if (!sensor_as_enabled2(i) || !sensor_as.param[i].use) {
        return false;
    }
    if (sensor_as.param[i].use == 2 && !math_flt_zero(srv_channels_get_output_scaled(k_throttle))) {
        // special case for gliders with airspeed sensors behind the
        // propeller. Allow airspeed to be disabled when throttle is
        // running
        return false;
    }
    return true;
}

void sensor_as_check_sensor_failures()
{
    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
        sensor_as_check_sensor_ahrs_wind_max_failures(i);
    }
}

void sensor_as_check_sensor_ahrs_wind_max_failures(uint8_t i)
{
    const uint32_t now_ms = time_millis();
    if ((now_ms - sensor_as.state[i].failures.last_check_ms) <= 200) {
        // slow the checking rate
        return;
    }
    sensor_as.state[i].failures.last_check_ms = now_ms;

    if (!math_flt_positive(sensor_as._wind_max) && !math_flt_positive(sensor_as._wind_gate)) {
        // nothing to do
        return;
    }

    if (sensor_as.state[i].airspeed <= 0) {
        // invalid estimate
        return;
    }

    uitc_sensor_gps vehicle_gps_position;
    itc_copy_from_hub(ITC_ID(sensor_gps),  &vehicle_gps_position);
    if (vehicle_gps_position.fix_type < uITC_GPS_OK_FIX_3D) {
        // GPS speed can't be trusted, re-enable airspeed as a fallback
        if ((sensor_as.param[i].use == 0) && (sensor_as.state[i].failures.param_use_backup == 1)) {
            GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Airspeed sensor %d, Re-enabled as GPS fall-back", i+1);

            param_t *param = param_get_by_variable2("ARSPD", (void*)&sensor_as.param[i].use);
            if (param != NULL) {
                param_set_obj_and_notify(param, sensor_as.state[i].failures.param_use_backup);
            }

            sensor_as.state[i].failures.param_use_backup = -1;
        }
        return;
    }

    // check for airspeed consistent with wind and vehicle velocity using the EKF
    uitc_wind wind = {0};
    if ((itc_publish(ITC_ID(wind), &wind) == 0) && math_flt_positive(wind.tas_innov_var)) {
        sensor_as.state[i].failures.test_ratio = fabsf(wind.tas_innov) / math_sqrtf(wind.tas_innov_var);
    } else {
        sensor_as.state[i].failures.test_ratio = 0.0f;
    }
    bool data_is_inconsistent = false;
    if (math_flt_positive(sensor_as._wind_gate) && (AS_OM_USE_EKF_CONSISTENCY & sensor_as._options) != 0) {
        float gate_size = MAX(sensor_as._wind_gate, 0.0f);
        if (sensor_as.param[i].use == 0) {
            // require a smaller inconsistency for a disabled sensor to be declared consistent
            gate_size *= 0.7f;
        }
        data_is_inconsistent = sensor_as.state[i].failures.test_ratio > gate_size;
    }
    
    const float gps_speed = vehicle_gps_position.vel_m_s;
    const float speed_diff = fabsf(sensor_as.state[i].airspeed-gps_speed);
    const bool data_is_implausible = math_flt_positive(sensor_as._wind_max) && speed_diff > sensor_as._wind_max;
    // update health_probability with LowPassFilter
    if (data_is_implausible || data_is_inconsistent) {
        // bad, decay fast
        const float probability_coeff = 0.90f;
        sensor_as.state[i].failures.health_probability = probability_coeff*sensor_as.state[i].failures.health_probability;

    } else if (!data_is_implausible && !data_is_inconsistent) {
        // good, grow slow
        const float probability_coeff = 0.98f;
        sensor_as.state[i].failures.health_probability = probability_coeff*sensor_as.state[i].failures.health_probability + (1.0f-probability_coeff)*1.0f;
    }

    // Now check if we need to disable or enable the sensor

    // here are some probability thresholds
    static const float DISABLE_PROB_THRESH_CRIT = 0.1f;
    static const float RE_ENABLE_PROB_THRESH_OK = 0.95f;

    if (sensor_as.param[i].use > 0) {
        if (((AS_OM_ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE & sensor_as._options) != 0) &&
                (sensor_as.state[i].failures.health_probability < DISABLE_PROB_THRESH_CRIT)) {
            // if "disable" option is allowed and sensor is enabled and is probably not healthy
            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Airspeed sensor %d failure. Disabling", i+1);
            sensor_as.state[i].failures.param_use_backup = sensor_as.param[i].use;

            param_t *param = param_get_by_variable2("ARSPD", (void*)&sensor_as.param[i].use);
            if (param != NULL) {
                param_set_obj_and_notify(param, 0);
            }
            sensor_as.state[i].healthy = false;
        }

        // Warn if necessary

        // set warn to max if not set or larger than max
        float wind_warn = sensor_as._wind_warn;
        if ((!math_flt_positive(wind_warn) || (wind_warn > sensor_as._wind_max)) && math_flt_positive(sensor_as._wind_max)) {
            wind_warn = sensor_as._wind_max;
        }

        if (math_flt_positive(wind_warn) && (speed_diff > wind_warn) && ((now_ms - sensor_as.state[i].failures.last_warn_ms) > 15000)) {
            sensor_as.state[i].failures.last_warn_ms = now_ms;
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Airspeed %d warning %0.1fm/s air to gnd speed diff", i+1, speed_diff);
        }

    // if Re-Enable options is allowed, and sensor is disabled but was previously enabled, and is probably healthy
    } else if (((AS_OM_ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE & sensor_as._options) != 0) &&
                (sensor_as.state[i].failures.param_use_backup > 0) && 
                (sensor_as.state[i].failures.health_probability > RE_ENABLE_PROB_THRESH_OK)) {

        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Airspeed sensor %d now OK. Re-enabled", i+1);

        param_t *param = param_get_by_variable2("ARSPD", (void*)&sensor_as.param[i].use);
        if (param != NULL) {
            param_set_obj_and_notify(param, sensor_as.state[i].failures.param_use_backup);
        }

        sensor_as.state[i].failures.param_use_backup = -1; // set to invalid so we don't use it
    }
}

void sensor_as_set_fixedwing_parameters(mb_fixed_wing_const_t _fixed_wing_parameters)
{
    sensor_as.fixed_wing_parameters = _fixed_wing_parameters;
}
/*------------------------------------test------------------------------------*/


